#include <cstdlib>
#include <iostream>
#include <libobsensor/hpp/Error.hpp>
#include <memory>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
 
#include "camera.h"

// 新增
#include "OpencvPreprocess.h"

cv::Mat rec_hsv;

// Define mouse callback function
void mouseColor(int event, int x, int y, int flags, void* param) {
    if (event == cv::EVENT_LBUTTONDOWN) {
        std::cout << "HSV: " << rec_hsv.at<cv::Vec3b>(y, x) << std::endl;
    }
}

int main(void) {
    try {
        Camera camera(true, true, true);
        camera.start();
        std::vector<cv::Point2f> centers;
        while (cv::waitKey(1) != 27) { // ESC的ASCII码是27，按ESC键可以终止程序
            auto frame_set = camera.get();
            if (frame_set != nullptr) {
                cv::Mat img = camera.frame2mat(frame_set->colorFrame());
                if (!img.empty()) {
                    cv::imshow("Color", img);
                    preprocess(img, centers);

                    // // 新增  测HSV
                    // cv::cvtColor(img, rec_hsv, cv::COLOR_BGR2HSV);  // Convert to HSV format
                    // cv::namedWindow("Color Picker");
                    // cv::setMouseCallback("Color Picker", mouseColor);
                    // cv::imshow("Color Picker", img);
                }
                img = camera.frame2mat(frame_set->irFrame());
                if (!img.empty()) {
                    cv::imshow("IR", img);
                }

                auto depthFrame = frame_set->depthFrame();
                img = camera.frame2mat(depthFrame);
                if (!img.empty()) {
                    cv::imshow("Depth", img);

                    // 新增
                    // Convert depth values to 1mm precision
                    camera.depthPrecisionConversion(depthFrame);
                    camera.get_depth(depthFrame, centers);
                }
            }
            centers.clear();
        }
        camera.stop();
        cv::destroyAllWindows();
    } catch (const ob::Error &e) {
        std::cerr << "Function:" << e.getName() << "\nargs:" << e.getArgs()
                  << "\nmessage:" << e.getMessage() << "\ntype:" << e.getExceptionType()
                  << std::endl;
        exit(EXIT_FAILURE);
    } catch (const std::exception &e) {
        std::cerr << e.what() << std::endl;
        exit(EXIT_FAILURE);
    } catch (...) {
        std::cerr << "Unexpected Error!" << std::endl;
        exit(EXIT_FAILURE);
    }
    return EXIT_SUCCESS;
}